/****************************************************************
 *
 * Project: IMU
 * 
 ****************************************************************/

/****************************************************************
 *
 *	Module		: main_Day4_SPI.c
 *  Tool		: KeilC ARM 4.22a
 *	Chip		: STM32F407VG
 * 	History		: 28/6/2012
 *	Description	: Day 12 
 				+ I2C 
				+ Read registers in LSM330 BOARD

 *				
 *	Author		: Dang Anh Tung			
 *	Description	: www.payitforward.edu.vn
 *				
 *
 ****************************************************************/
 

 /****************************************************************
 * 							IMPORT
 ****************************************************************/
#include "stm32f4_discovery.h"
#include "stm32f4xx_conf.h"
#include "main.h"

 #include <stdio.h>                     /* This ert_main.c example uses printf/fflush */
#include "TungSim.h"

/*****************************************************************
* 							DEFINE
******************************************************************/
#define PI          (REAL)3.1415f

#define ACC_SCALE       (REAL)0.002
#define ACC_X_SCALE     (REAL)1/8192
#define ACC_Y_SCALE     (REAL)1/8192
#define ACC_Z_SCALE     (REAL)1/8192

#define GYR_SCALE       (REAL)0.00875 * PI /180 /* degree/s -> rad/s */
#define GYR_OFFSET_X    (REAL)0
#define GYR_OFFSET_Y    (REAL)0
#define GYR_OFFSET_Z    (REAL)0

#define MAG_SCALE       (REAL)1/1090
#define MAG_OFFSET_X    (REAL)0.00  //0.01
#define MAG_OFFSET_Y    (REAL)0.14  //0.15
#define MAG_OFFSET_Z    (REAL)0

#define MSG_RAW_DATA_INT    (uint8_t)1
#define MSG_RAW_DATA_REAL   (uint8_t)2
#define MSG_RAW_ANGLE_REAL  (uint8_t)3

/*****************************************************************
* 					 GLOBAL VARIABLE
******************************************************************/
extern uint32_t systick_counter;
static boolean_T OverrunFlag = 0;

/* Exported block signals */
extern real_T fgyro[3];                       /* '<Root>/Gyro  Sensor' */
extern real_T facc[3];                        /* '<Root>/Acc  Sensor' */
extern real_T fmag[3];                        /* '<Root>/Mag  Sensor' */
extern real_T Out1[3];                        /* '<Root>/Gain2' */
extern real_T Out2;                           /* '<Root>/Pulse Generator' */

#ifdef _USE_CALIB_			

int16_t Acc_X_Scale;			  
int16_t Acc_Y_Scale;
int16_t Acc_Z_Scale;
int16_t Acc_X_Offset;
int16_t Acc_Y_Offset;
int16_t Acc_Z_Offset;

int16_t Gyr_X_Offset;
int16_t Gyr_Y_Offset;
int16_t Gyr_Z_Offset;

int16_t Mag_X_Offset;
int16_t Mag_Y_Offset;

#endif


							  
/*****************************************************************
* 					SUBROUTINE LIST
******************************************************************/
void mySystemInit(void);
void mySysTickInit(void);
#ifdef _USE_CALIB_
void myIMUCalib(void);
#endif
void Delay(void);
void rt_OneStep(void);

/****************************************************************
*						INTERRUPT								*					
*****************************************************************/



 /****************************************************************
 * 						MAIN PROGRAM
 ****************************************************************/	
int main(void)
{
    static uint8_t Frame[255];
    static uint8_t FrameEst[255];
    uint32_t i;
	uint8_t readBuffer[10];
    /* Acc Parameter Init */
    uint8_t Acc_ConfigAddr[7]  = {0x20, 0x21, 0x22, 0x23, 0x24, 0x25, 0x2E};
    uint8_t Acc_ConfigValue[7] = {0xA7, 0x00, 0x00, 0x10, 0x00, 0x00, 0x00};
    /* Gyr Parameter Init */
    uint8_t Gyr_ConfigAddr[6]  = {0x20, 0x21, 0x22, 0x23, 0x24, 0x2E};
    uint8_t Gyr_ConfigValue[6] = {0x3F, 0x00, 0x00, 0x10, 0x13, 0x00};
    /* Mag Parameter Init */
    uint8_t Mag_ConfigAddr[3]  = {0x00, 0x01, 0x02};
    uint8_t Mag_ConfigValue[3] = {0x78, 0x20, 0x01};        //{0x78, 0x20, 0x01};

    uint8_t data[6];
    uint8_t bitStatus;
    static int16_t ix_,iy_,iz_;
    static REAL ax,ay,az, gx,gy,gz, mx,my,mz;
    static REAL phi, theta, psi, phi_est, theta_est, psi_est;

    static uint32_t N = 0;

    uint32_t SendEnable;

	mySystemInit();                

    /*-------------------------------------- 
    * TEST I2C READ ONE/MULTY BYTE[s] 
    ---------------------------------------*/
    /* Magnetometer */
	myI2CReadBytes(I2C_SOURCE, &readBuffer[0], I2C_MAG_WRITE, I2C_MAG_ID_REGA, 1);
    myI2CReadBytes(I2C_SOURCE, &readBuffer[1], I2C_MAG_WRITE, I2C_MAG_ID_REGA, 3);

    /* Accelerometer */
    myI2CReadBytes(I2C_SOURCE, &readBuffer[4], I2C_ACC_WRITE, I2C_ACC_CTRL_REG1, 1);
    myI2CReadBytes(I2C_SOURCE, &readBuffer[4], I2C_ACC_WRITE, I2C_ACC_CTRL_REG1, 3);

    /* Gyrometer */
    myI2CReadBytes(I2C_SOURCE, &readBuffer[5], I2C_GYR_WRITE, I2C_GYR_CTRL_REG1, 1);
    
    /*-------------------------------------- 
    * TEST I2C WRITE ONE/MULTY BYTE[s]
    * INITIALIZATION SENSOR 
    ---------------------------------------*/
    /* Accelerometer */ 
//    myI2CWriteBytes(I2C_SOURCE, &Acc_ConfigValue[0], I2C_ACC_WRITE, Acc_ConfigAddr[0], 6);
//    myI2CWriteBytes(I2C_SOURCE, &Acc_ConfigValue[6], I2C_ACC_WRITE, Acc_ConfigAddr[6], 1);
    for (i = 0; i < 7 ; i++)
    {
        myI2CWriteBytes(I2C_SOURCE, &Acc_ConfigValue[i], I2C_ACC_WRITE, Acc_ConfigAddr[i], 1);
    }
    
    /* Gyrometer */
    for (i = 0; i < 6  ; i++)
    {
        myI2CWriteBytes(I2C_SOURCE, &Gyr_ConfigValue[i], I2C_GYR_WRITE, Gyr_ConfigAddr[i], 1);
    }     

    /* Magnetometer */
    myI2CWriteBytes(I2C_SOURCE, &Mag_ConfigValue[0], I2C_MAG_WRITE, Mag_ConfigAddr[0], 3);

#ifdef _USE_CALIB_
    myIMUCalib();
#endif

    /* Init DCM model */
    TungSim_initialize();
    SendEnable = TRUE;
    while(1)
    {
        if(GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_0) == Bit_SET)
        {
            /* Wait until key is released */
            while(GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_0) == Bit_SET);
            if (SendEnable == TRUE) {SendEnable = FALSE;}   
            else {SendEnable = TRUE;}
        }      

        if (SendEnable == TRUE)
        {
            /* Reset counter */
            systick_counter = 0;      
            /* TEST ACC */
            myI2CReadBytes(I2C_SOURCE, &bitStatus, I2C_ACC_WRITE, I2C_ACC_STATUS_REG, 1);
            if ((bitStatus & 0x08) == 0x08)
            {
                //myI2CReadBytes(I2C_SOURCE, &data[0], I2C_ACC_WRITE, I2C_ACC_OUT_X_L, 6);
                for (i = 0; i < 6 ; i++)
                {
                    myI2CReadBytes(I2C_SOURCE, &data[i], I2C_ACC_WRITE, I2C_ACC_OUT_X_L + i, 1);
                }
    
                ix_ = myTwoComp2Int(&data[1], &data[0]);
                iy_ = myTwoComp2Int(&data[3], &data[2]);
                iz_ = myTwoComp2Int(&data[5], &data[4]);
    
    #ifdef _USE_CALIB_
                ax = (REAL)(ix_ - Acc_X_Offset) / Acc_X_Scale;
                ay = (REAL)(iy_ - Acc_Y_Offset) / Acc_Y_Scale;
                az = (REAL)(iz_ - Acc_Z_Offset) / Acc_Z_Scale;
    #else
                ax = (REAL)ix_ * ACC_X_SCALE;     
                ay = (REAL)iy_ * ACC_Y_SCALE; 
                az = (REAL)iz_ * ACC_Z_SCALE;
    #endif
                
            }
           
            /* TEST GYR */
            myI2CReadBytes(I2C_SOURCE, &bitStatus, I2C_GYR_WRITE, I2C_GYR_STATUS_REG, 1);
            if ((bitStatus & 0x08) == 0x08)
            {
                //myI2CReadBytes(I2C_SOURCE, &data[0], I2C_GYR_WRITE, I2C_GYR_OUT_X_L, 6);
                for (i = 0; i < 6 ; i++)
                {
                    myI2CReadBytes(I2C_SOURCE, &data[i], I2C_GYR_WRITE, I2C_GYR_OUT_X_L+i, 1);
                }
    
                ix_ = myTwoComp2Int(&data[1], &data[0]);
                iy_ = myTwoComp2Int(&data[3], &data[2]);
                iz_ = myTwoComp2Int(&data[5], &data[4]);    
    
    #ifdef _USE_CALIB_           
                gx = (REAL)(ix_ - Gyr_X_Offset) * GYR_SCALE;                    
                gy = (REAL)(iy_ - Gyr_Y_Offset) * GYR_SCALE;
                gz = (REAL)(iz_ - Gyr_Z_Offset) * GYR_SCALE;
    #else
                gx = (REAL)ix_ * GYR_SCALE;                    
                gy = (REAL)iy_ * GYR_SCALE;
                gz = (REAL)iz_ * GYR_SCALE;
    #endif
            }
    
            /* TEST MAG */
            myI2CReadBytes(I2C_SOURCE, &bitStatus, I2C_MAG_WRITE, I2C_MAG_STATUS_REG, 1);
            if ((bitStatus & 0x01) == 0x01)     
            {
                myI2CReadBytes(I2C_SOURCE, &data[0], I2C_MAG_WRITE, I2C_MAG_OUT_X_H, 6);
                /*  
                *   !!!!!!!! NOTE !!!!!!!
                *   After perform new data, HMC return to sleep mode. 
                *   So that We must re-configure it to single measurement mode for next data 
                *   Get data in the next cycle (delay several ms)
                */ 
                myI2CWriteBytes(I2C_SOURCE, &Mag_ConfigValue[2], I2C_MAG_WRITE, Mag_ConfigAddr[2], 1);
    
                ix_ = myTwoComp2Int(&data[0], &data[1]);
                iz_ = myTwoComp2Int(&data[2], &data[3]);        /* Note this is z-axis */
                iy_ = myTwoComp2Int(&data[4], &data[5]);  
    
    #ifdef _USE_CALIB_
                mx = (REAL)(ix_ - Mag_X_Offset) * MAG_SCALE;
                my = (REAL)(iy_ - Mag_Y_Offset) * MAG_SCALE;
                mz = (REAL)iz_ * MAG_SCALE;
    #else
                mx = (REAL)ix_ * MAG_SCALE + MAG_OFFSET_X;
                my = (REAL)iy_ * MAG_SCALE + MAG_OFFSET_Y;
                mz = (REAL)iz_ * MAG_SCALE + MAG_OFFSET_Z;
    #endif
            }
    
            phi = asin (ay/sqrt(ay * ay + az * az)) * 180 / PI;
            theta = - asin(ax) * 180 / PI;        /* a.x/g but a.x is divided by 9.8 already */
            psi = atan(my/mx) * 180 / PI;
            
//            Frame[0] = 255;
//            Frame[1] = MSG_RAW_DATA_REAL;
//            Frame[2] = 36;
//            myReal2Bytes(ax, &Frame[3]);
//            myReal2Bytes(ay, &Frame[7]);
//            myReal2Bytes(az, &Frame[11]);
//            myReal2Bytes(gx, &Frame[15]);
//            myReal2Bytes(gy, &Frame[19]);
//            myReal2Bytes(gz, &Frame[23]);
//            myReal2Bytes(mx, &Frame[27]);
//            myReal2Bytes(my, &Frame[31]);
//            myReal2Bytes(mz, &Frame[35]);
//            Frame[39] = 254;
    
            //myUsartSendBytes(USART1, &Frame[0], 40);
            //myUsartSendBytes(USART1, &Message, 14);
            //myUsartSendDma(DMA_USART1_TX_STREAM, USART1, &Frame[0], 40);
    //        myBytes2Real(&x_test, &Frame[3]);
    //        myBytes2Real(&y_test, &Frame[7]);
    //        myBytes2Real(&z_test, &Frame[11]);
            

            fgyro[0] = gx;
            fgyro[1] = gy;
            fgyro[2] = gz;

            facc[0] = ax;
            facc[1] = ay;
            facc[2] = az;

            fmag[0] = mx;
            fmag[1] = my;
            fmag[2] = mz;

//            z[0] = ax;
//            z[1] = ay;
//            z[2] = az;
//            z[3] = gx;
//            z[4] = gy;
//            z[5] = gz;
//            z[6] = mx;
//            z[7] = my;
//            z[8] = mz;
            
            /* MATLAB Process */
            rt_OneStep();
    
            phi_est   = Out1[0] * 180 / PI;
            theta_est = Out1[1] * 180 / PI;
            psi_est   = Out1[2] * 180 / PI;            

            myReal2Str(phi_est, &FrameEst[0], 3, 1);
            FrameEst[6]  = ' ';
            myReal2Str(theta_est, &FrameEst[7], 3, 1);
            FrameEst[13] = ' ';
            myReal2Str(psi_est, &FrameEst[14], 3, 1);

            FrameEst[20] = ' ';
            myReal2Str(gx, &FrameEst[21], 1, 3);
            FrameEst[27] = ' ';
            myReal2Str(gy, &FrameEst[28], 1, 3);
            FrameEst[34] = ' ';
            myReal2Str(gz, &FrameEst[35], 1, 3);

            FrameEst[41] = ' ';
            myReal2Str(ax, &FrameEst[42], 1, 3);
            FrameEst[48] = ' ';
            myReal2Str(ay, &FrameEst[49], 1, 3);
            FrameEst[55] = ' ';
            myReal2Str(az, &FrameEst[56], 1, 3);

            FrameEst[62] = ' ';
            myReal2Str(mx, &FrameEst[63], 1, 3);
            FrameEst[69] = ' ';
            myReal2Str(my, &FrameEst[70], 1, 3);
            FrameEst[76] = ' ';
            myReal2Str(mz, &FrameEst[77], 1, 3);

            FrameEst[83] = '\r';
            FrameEst[84] = '\n';

            myUsartSendDma(DMA_USART1_TX_STREAM, USART1, &FrameEst[0], 85);
            N++;

            while(systick_counter <= 10);   /* Waiting for  this loop end in 10ms */
        }    
     }
}		


 /****************************************************************
 * 						SUBROUTINEs
 ****************************************************************/	

/****************************************************************
** Function name:		myIMUCalib
**
** Descriptions:		Get some value: 
**                         @ Max & Min Acce 3 axis when IMU rotate in each axis
**                         @ Offset Gyr 3 axis when IMU stand not move
**                         @ Max & Min Mag x,y when IMU rotate in z-axis
**
** parameters:			None
** Returned value:		None
** 
*****************************************************************/
#ifdef _USE_CALIB_
void myIMUCalib(void)
{
    int16_t Max = 0, Min = 0;
    uint8_t data[6];     
    int16_t i_,ix_,iy_,iz_;
    uint8_t bitStatus;
    uint32_t i;
    int16_t n;

    /* Mag Parameter Init */
    uint8_t Mag_ConfigAddr[3]  = {0x00, 0x01, 0x02};
    uint8_t Mag_ConfigValue[3] = {0x78, 0x20, 0x01};        //{0x78, 0x20, 0x01};

    /*------------------------------------------------------
    *   Acc_Scale  = (Acc_Max - Acc_Min)/2;                *
    *   Acc_Offset = (Acc_Max + Acc_Min)/2;                *
    *   Acc_Calib  = (Acc_Raw - Acc_Offset)/Acc_Scale;     *
    --------------------------------------------------------*/

    /* 
    * Calib Acc in Z Axis 
    * Rotate Acc in Z axis
    */
    Max = 0;
    Min = 0;
    while(GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_0) == Bit_RESET)
    {        
        myI2CReadBytes(I2C_SOURCE, &bitStatus, I2C_ACC_WRITE, I2C_ACC_STATUS_REG, 1);
        if ((bitStatus & 0x08) == 0x08)
        {
            for (i = 0; i < 2 ; i++)
            {
                myI2CReadBytes(I2C_SOURCE, &data[i], I2C_ACC_WRITE, I2C_ACC_OUT_Z_L + i, 1);
            }
            i_ = myTwoComp2Int(&data[1], &data[0]);
            if ((i_ > Max) && (i_ <= 8192)) Max = i_;
            if ((i_ < Min) && (i_ >= -8192)) Min = i_;
        }
    }
    while(GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_0) == Bit_SET); /* Wait until key is released */
    Acc_Z_Scale  = (Max - Min)/2;
    Acc_Z_Offset = (Max + Min)/2;


    /* 
    * Calib Acc in X Axis 
    * Rotate Acc in X axis
    */
    Max = 0;
    Min = 0;
    while(GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_0) == Bit_RESET)
    {        
        myI2CReadBytes(I2C_SOURCE, &bitStatus, I2C_ACC_WRITE, I2C_ACC_STATUS_REG, 1);
        if ((bitStatus & 0x08) == 0x08)
        {
            for (i = 0; i < 2 ; i++)
            {
                myI2CReadBytes(I2C_SOURCE, &data[i], I2C_ACC_WRITE, I2C_ACC_OUT_X_L + i, 1);
            }
            i_ = myTwoComp2Int(&data[1], &data[0]);
            if ((i_ > Max) && (i_ <= 8192)) Max = i_;
            if ((i_ < Min) && (i_ >= -8192)) Min = i_;
        }
    }    
    while(GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_0) == Bit_SET); /* Wait until key is released */
    Acc_X_Scale  = (Max - Min)/2;
    Acc_X_Offset = (Max + Min)/2;


    /* 
    * Calib Acc in Y Axis 
    * Rotate Acc in Y axis
    */
    Max = 0;
    Min = 0;
    while(GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_0) == Bit_RESET)
    {        
        myI2CReadBytes(I2C_SOURCE, &bitStatus, I2C_ACC_WRITE, I2C_ACC_STATUS_REG, 1);
        if ((bitStatus & 0x08) == 0x08)
        {
            for (i = 0; i < 2 ; i++)
            {
                myI2CReadBytes(I2C_SOURCE, &data[i], I2C_ACC_WRITE, I2C_ACC_OUT_Y_L + i, 1);
            }
            i_ = myTwoComp2Int(&data[1], &data[0]);
            if ((i_ > Max) && (i_ <= 8192)) Max = i_;
            if ((i_ < Min) && (i_ >= -8192)) Min = i_;
        }
    }    
    while(GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_0) == Bit_SET); /* Wait until key is released */
    Acc_Y_Scale  = (Max - Min)/2;
    Acc_Y_Offset = (Max + Min)/2;

    /* 
    * Calib Gyr in 3 axis
    * Let IMU stand
    */
    n = 1;
    while(GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_0) == Bit_RESET)
    {        
        myI2CReadBytes(I2C_SOURCE, &bitStatus, I2C_GYR_WRITE, I2C_GYR_STATUS_REG, 1);
        if ((bitStatus & 0x08) == 0x08)
        {
            for (i = 0; i < 6 ; i++)
            {
                myI2CReadBytes(I2C_SOURCE, &data[i], I2C_GYR_WRITE, I2C_GYR_OUT_X_L+i, 1);
            }

            ix_ = myTwoComp2Int(&data[1], &data[0]);
            iy_ = myTwoComp2Int(&data[3], &data[2]);
            iz_ = myTwoComp2Int(&data[5], &data[4]);    
            
            /* 
            * Algorithm for calculate mean of array numbers 
            * t_n = t_n_1 * (n - 1) / n + a_n / n;
            * Where: 
            *   n from 1->infinite; 
            *   a_n: input value at time n; 
            *   t_n: mean value at time n;
            */
            Gyr_X_Offset = ( Gyr_X_Offset * (n - 1) + ix_ ) / n;
            Gyr_Y_Offset = ( Gyr_Y_Offset * (n - 1) + iy_ ) / n;
            Gyr_Z_Offset = ( Gyr_Z_Offset * (n - 1) + iz_ ) / n;
            n++;
        }
    }    
    while(GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_0) == Bit_SET); /* Wait until key is released */
    
    /* 
    * Calib Mag in 2 axis X
    * Let IMU rotate in z-axis
    */
    Max = 0;
    Min = 0;
    while(GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_0) == Bit_RESET)
    {        
        myI2CReadBytes(I2C_SOURCE, &bitStatus, I2C_MAG_WRITE, I2C_MAG_STATUS_REG, 1);
        if ((bitStatus & 0x01) == 0x01)     
        {
            myI2CReadBytes(I2C_SOURCE, &data[0], I2C_MAG_WRITE, I2C_MAG_OUT_X_H, 2);
            /*  
            *   !!!!!!!! NOTE !!!!!!!
            *   After perform new data, HMC return to sleep mode. 
            *   So that We must re-configure it to single measurement mode for next data 
            *   Get data in the next cycle (delay several ms)
            */ 
            myI2CWriteBytes(I2C_SOURCE, &Mag_ConfigValue[2], I2C_MAG_WRITE, Mag_ConfigAddr[2], 1);
            Delay();
            i_ = myTwoComp2Int(&data[0], &data[1]);
            if ((i_ > Max) && (i_ < 358)) Max = i_;     /* 358 = 1024/1.2*0.42: Mag of Vietnam maximum is 0.41*/
            if ((i_ < Min) && (i_ > -358)) Min = i_;
        }
    }    
    while(GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_0) == Bit_SET); /* Wait until key is released */
    Mag_X_Offset = (Max + Min)/2;

    /* 
    * Calib Mag in 2 axis Y
    * Let IMU rotate in z-axis
    */
    Max = 0;
    Min = 0;
    while(GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_0) == Bit_RESET)
    {        
        myI2CReadBytes(I2C_SOURCE, &bitStatus, I2C_MAG_WRITE, I2C_MAG_STATUS_REG, 1);
        if ((bitStatus & 0x01) == 0x01)     
        {
            myI2CReadBytes(I2C_SOURCE, &data[0], I2C_MAG_WRITE, I2C_MAG_OUT_X_H, 2);
            /*  
            *   !!!!!!!! NOTE !!!!!!!
            *   After perform new data, HMC return to sleep mode. 
            *   So that We must re-configure it to single measurement mode for next data 
            *   Get data in the next cycle (delay several ms)
            */ 
            myI2CWriteBytes(I2C_SOURCE, &Mag_ConfigValue[2], I2C_MAG_WRITE, Mag_ConfigAddr[2], 1);
            Delay();
            i_ = myTwoComp2Int(&data[0], &data[1]);
            if ((i_ > Max) && (i_ < 358)) Max = i_;     /* 358 = 1024/1.2*0.42: Mag of Vietnam maximum is 0.41*/
            if ((i_ < Min) && (i_ > -358)) Min = i_;
        }
    }    
    while(GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_0) == Bit_SET); /* Wait until key is released */
    Mag_Y_Offset = (Max + Min)/2;

} 
#endif /* _USE_CALIB_ */

/****************************************************************
** Function name:		SystemInit
**
** Descriptions:		Initiate system
**
** parameters:			None
** Returned value:		None
** 
*****************************************************************/
void mySystemInit(void)
{
    GPIO_InitTypeDef  GPIO_InitStructure;

	myI2CInit();
    mySysTickInit();
    myUsartInit();
    myDmaUsartInit();
    myDmaUsartInterruptInit();
//    myDmaI2CInit();
//    myDmaI2CInterruptInit();	


	/* GPIOD Periph clock enable */
  	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);
	/* Configure PD12, PD13, PD14 and PD15 in output pushpull mode */
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12 | GPIO_Pin_13| GPIO_Pin_14| GPIO_Pin_15;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
	GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
	GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
	GPIO_Init(GPIOD, &GPIO_InitStructure);

    // GPIOA Periph clock enable
	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);
	// configure button PA0 in input mode
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
	GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
	GPIO_Init(GPIOD, &GPIO_InitStructure);
}	


void mySysTickInit(void)
{
    NVIC_InitTypeDef NVIC_InitStruct;

	SysTick_Config(21000); 		//CMSIS library defined 1 tick = 21000 = 168M/8 * 1ms
	SysTick_CLKSourceConfig(SysTick_CLKSource_HCLK_Div8);

	/*enable System Tick timer*/
	NVIC_InitStruct.NVIC_IRQChannel = SysTick_IRQn;
	NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority = 0;			//highest priority
	NVIC_InitStruct.NVIC_IRQChannelSubPriority = 0;					//sub-priority between many interrupt sources
	NVIC_InitStruct.NVIC_IRQChannelCmd = ENABLE;
	NVIC_Init(&NVIC_InitStruct);
}


/****************************************************************
** Function name:		Delay
**
** Descriptions:		loop in certain time before continue next step
**
** parameters:			None
** Returned value:		None
** 
*****************************************************************/
void Delay()
{
	uint32_t count = 16800000;
	while(count--);
}

void rt_OneStep(void)
{
  static boolean_T OverrunFlag = 0;

  /* Disable interrupts here */

  /* Check for overrun */
  if (OverrunFlag) {
    rtmSetErrorStatus(TungSim_M, "Overrun");
    return;
  }

  OverrunFlag = TRUE;

  /* Save FPU context here (if necessary) */
  /* Re-enable timer or interrupt here */
  /* Set model inputs here */

  /* Step the model */
  TungSim_step();

  /* Get model outputs here */

  /* Indicate task complete */
  OverrunFlag = FALSE;

  /* Disable interrupts here */
  /* Restore FPU context here (if necessary) */
  /* Enable interrupts here */
}

 


/****************************************************************
 * END OF standard form.c
 ****************************************************************/
